#include <iostream>

#include "SystemSetting.h"

using namespace std;

namespace ORB_SLAM2
{

SystemSetting::SystemSetting(ORBVocabulary* pVoc):
    pVocabulary(pVoc)
{
}

//SystemSetting::SystemSetting(ORBVocabulary* pVoc, KeyFrameDatabase* pKFDB):
//    pVocabulary(pVoc), pKeyFrameDatabase(pKFDB)
//    {
//    }


bool SystemSetting::LoadSystemSetting(const std::string strSettingPath)
{
    cout<<endl<<"Loading System Parameters form:"<<strSettingPath<<endl;
    cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
    width  = fSettings["Camera.width"];
    height = fSettings["Camera.height"];
    fx     = fSettings["Camera.fx"];
    fy     = fSettings["Camera.fy"];
    cx     = fSettings["Camera.cx"];
    cy     = fSettings["Camera.cy"];

    cv::Mat tmpK = cv::Mat::eye(3,3,CV_32F);
    tmpK.at<float>(0,0) = fx;
    tmpK.at<float>(1,1) = fy;
    tmpK.at<float>(0,2) = cx;
    tmpK.at<float>(1,2) = cy;
    tmpK.copyTo(K);

    cv::Mat tmpDistCoef(4,1,CV_32F);
    tmpDistCoef.at<float>(0) = fSettings["Camera.k1"];
    tmpDistCoef.at<float>(1) = fSettings["Camera.k2"];
    tmpDistCoef.at<float>(2) = fSettings["Camera.p1"];
    tmpDistCoef.at<float>(3) = fSettings["Camera.p2"];
    const float k3 = fSettings["Camera.k3"];
    if( k3!=0 )
    {
        tmpDistCoef.resize(5);
        tmpDistCoef.at<float>(4) = k3;
    }
    tmpDistCoef.copyTo( DistCoef );

    bf = fSettings["Camera.bf"];
    fps= fSettings["Camera.fps"];

    invfx = 1.0f/fx;
    invfy = 1.0f/fy;
    b     = bf  /fx;
    initialized = true;

    cout<<"- size:"<<width<<"x"<<height<<endl;
    cout<<"- fx:"  <<fx<<endl;
    cout << "- fy: " << fy << endl;
    cout << "- cx: " << cx << endl;
    cout << "- cy: " << cy << endl;
    cout << "- k1: " << DistCoef.at<float>(0) << endl;
    cout << "- k2: " << DistCoef.at<float>(1) << endl;
    if(DistCoef.rows==5)
        cout << "- k3: " << DistCoef.at<float>(4) << endl;
    cout << "- p1: " << DistCoef.at<float>(2) << endl;
    cout << "- p2: " << DistCoef.at<float>(3) << endl;
    cout << "- bf: " << bf << endl;

    //Load RGB parameter
    nRGB = fSettings["Camera.RGB"];

    //Load ORB feature parameters
    nFeatures = fSettings["ORBextractor.nFeatures"];
    fScaleFactor = fSettings["ORBextractor.scaleFactor"];
    nLevels = fSettings["ORBextractor.nLevels"];
    fIniThFAST = fSettings["ORBextractor.iniThFAST"];
    fMinThFAST = fSettings["ORBextractor.minThFAST"];

    cout << endl  << "ORB Extractor Parameters: " << endl;
    cout << "- Number of Features: " << nFeatures << endl;
    cout << "- Scale Levels: " << nLevels << endl;
    cout << "- Scale Factor: " << fScaleFactor << endl;
    cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
    cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;

    //Load others parameters, if the sensor is MONOCULAR, the parameters is zero;
    //ThDepth = fSettings["ThDepth"];
    //DepthMapFactor = fSettings["DepthMapFactor"];
    fSettings.release();
    return true;
}

}   // namespace ORB_SLAM2
